﻿#include "algothread.h"

#include <QMessageBox>
#include<iostream>

using namespace std;
AlgoThread::AlgoThread(QThread *parent) : QThread (parent)
{

}

void AlgoThread::run()
{
//    if(!planner)
    {
//        planner = new Plan();
        connect(SignalFactory::SignalInstance().get(), SIGNAL(StartPlan(double, double, double, double)), this, SLOT(Plan(double, double, double, double)), Qt::QueuedConnection);
    }

}

void AlgoThread::Plan(double sx, double sy, double ex, double ey)
{


//std::cout<<"dao le Plan这个函数里面"<<endl;


    //设置地图长宽
//    int rows = 2001;        //Y轴宽
//    int columns = 801;     //X轴长

    int rows = 3470;        //Y轴宽
    int columns = 3468;     //X轴长
    int rowWeight=1735;
    int columnsWeight=1734;

    std::cout<<"准备进入maze"<<endl;
    vector<vector<int>> maze(columns, vector<int>(rows));
    std::cout<<"maze创建成功"<<endl;
    for(int i = 0; i < columns; i++)
    {
        for(int j = 0; j < rows; j++)
        {
            maze[i][j] = 0;     //0为可通行，1为障碍，初始化地图为全部可通行
        }
    }


    for(int k = 45+columnsWeight; k <= 49+columnsWeight; k++)
    {
        for(int b = 11+rowWeight; b <= 16+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }


    for(int k = 10+columnsWeight; k <= 15+columnsWeight; k++)
    {
        for(int b = 0+rowWeight; b <= 3+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }
    std::cout<<"地图初始一半"<<endl;

    for(int k = 44+columnsWeight; k <= 50+columnsWeight; k++)
    {
        for(int b = 15+rowWeight; b <= 20+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }
    for(int k = 58+columnsWeight; k <= 62+columnsWeight; k++)
    {
        for(int b = 35+rowWeight; b <= 40+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }
    for(int k = 22+columnsWeight; k <= 26+columnsWeight; k++)
    {
        for(int b = -2+rowWeight; b <= 2+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }
    for(int k = 63+columnsWeight; k <= 67+columnsWeight; k++)
    {
        for(int b = 37+rowWeight; b <= 39+rowWeight; b++)
        {
            maze[b][k] = 1;
        }
    }

//    maze[18][47] = 1;
//    maze[0][13] = 1;
//    maze[0][12] = 1;
//    maze[0][1] = 1;



    //在二维数组中置1设置障碍
    for(int k = 0; k <= 40; k++)
    {
        for(int b = 130; b <= 140; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 130; k++)
    {
        for(int b = 190; b <= 200; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 150; k++)
    {
        for(int b = 220; b <= 230; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 140; k++)
    {
        for(int b = 230; b <= 240; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 280; k++)
    {
        for(int b = 240; b <= 250; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 350; k++)
    {
        for(int b = 250; b <= 260; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 360; k++)
    {
        for(int b = 260; b <= 270; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 380; k++)
    {
        for(int b = 270; b <= 280; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 380; k++)
    {
        for(int b = 280; b <= 290; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 390; k++)
    {
        for(int b = 290; b <= 300; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 390; k++)
    {
        for(int b = 300; b <= 310; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 770; k++)
    {
        for(int b = 310; b <= 320; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 730; k++)
    {
        for(int b = 330; b <= 340; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 730; k++)
    {
        for(int b = 340; b <= 350; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 710; k++)
    {
        for(int b = 350; b <= 360; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 720; k++)
    {
        for(int b = 360; b <= 370; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 10; k <= 720; k++)
    {
        for(int b = 370; b <= 380; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 700; k++)
    {
        for(int b = 380; b <= 390; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 730; k++)
    {
        for(int b = 390; b <= 400; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int b = 400; b <= 410; b++)
    {
        for(int k = 690; k <= 700; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 710; k <= 720; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 760; k <= 770; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 830; k <= 840; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 850; k <= 860; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 910; k <= 920; k++)
        {
            maze[b][k] = 1;
        }

        for(int k = 960; k <= 970; k++)
        {
            maze[b][k] = 1;
        }

    }


    /*maze[40][71] = 1;
    maze[40][76] = 1;
    maze[40][83] = 1;
    maze[40][85] = 1;
    maze[40][91] = 1;
    maze[40][96] = 1;*/

    for(int k = 470; k <= 640; k++)
    {
        for(int b = 530; b <= 540; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1260; k <= 1280; k++)
    {
        for(int b = 530; b <= 540; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 470; k <= 640; k++)
    {
        for(int b = 540; b <= 550; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1270; k <= 1320; k++)
    {
        for(int b = 540; b <= 550; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 620; k++)
    {
        for(int b = 550; b <= 560; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1030; k <= 1310; k++)
    {
        for(int b = 580; b <= 590; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 540; k++)
    {
        for(int b = 590; b <= 600; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1030; k <= 1340; k++)
    {
        for(int b = 590; b <= 600; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1040; k <= 1360; k++)
    {
        for(int b = 600; b <= 610; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1040; k <= 1350; k++)
    {
        for(int b = 610; b <= 620; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1030; k <= 1380; k++)
    {
        for(int b = 620; b <= 630; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1030; k <= 1380; k++)
    {
        for(int b = 630; b <= 640; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1050; k <= 1380; k++)
    {
        for(int b = 640; b <= 650; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1050; k <= 1390; k++)
    {
        for(int b = 650; b <= 660; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 560; k++)
    {
        for(int b = 660; b <= 670; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1060; k <= 1400; k++)
    {
        for(int b = 660; b <= 670; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1070; k <= 1410; k++)
    {
        for(int b = 670; b <= 680; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1040; k <= 1410; k++)
    {
        for(int b = 680; b <= 690; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 1060; k <= 1410; k++)
    {
        for(int b = 690; b <= 700; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1410; k++)
    {
        for(int b = 700; b <= 710; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1440; k++)
    {
        for(int b = 710; b <= 720; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1420; k++)
    {
        for(int b = 720; b <= 730; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 730; b <= 740; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1340; k++)
    {
        for(int b = 740; b <= 750; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 750; b <= 760; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 760; b <= 770; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 770; b <= 780; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 780; b <= 790; b++)
        {
            maze[b][k] = 1;
        }
    }

    for(int k = 0; k <= 1390; k++)
    {
        for(int b = 790; b <= 800; b++)
        {
            maze[b][k] = 1;
        }
    }

    //边界
    /*for(int k = 0; k <= 1390; k++)
    {
        for(int b = 800; b <= 800; b++)
        {
            maze[b][k] = 1;
        }
    }*/

    Astar astar;
//    std::cout<<"准备进入Astar算法"<<endl;
    astar.InitAstar(maze);
//    std::cout<<"从Astar initAstar出来了"<<endl;


    //设置起始点
//    Point start(400,1000);
//    Point end(430,1300);
//    int rows = 3470;        //Y轴宽
//    int columns = 3468;
    Point start(sx + 1735, sy + 1734);
    Point end(ex + 1735, ey + 1734);
    if(maze[sx + 1735][sy + 1734]==1||maze[ex + 1735][ey + 1734]==1){
        emit SignalFactory::SignalInstance()->PlanFailInfo();
        return;
    }


// if((maze[sx][sy] != 1) && (maze[ex][ey] != 1))
// {
     list<Point *> path = astar.GetPath(start , end, false);
     list<Point *>::iterator n;
     unsigned long long i = 0;
//     unsigned long long i2 = 0;
     SignalFactory::SignalInstance()->m_PointNum = path.size();
 //    m_PointNum = pointNum;
//     double point[5000];
     //打印路径上的节点
 //    for(auto &p:path)
 //        cout<<'('<<(p->x) - 400<<','<<(p->y) - 1000<<')'<<endl;

     for(n = path.begin(); i < SignalFactory::SignalInstance()->m_PointNum ; i++ ,++n)
     {

//         qDebug()<< '('<<((*n)->x) - 400<<','<<((*n)->y) - 1000<<')'<<endl;
         SignalFactory::SignalInstance()->locateX[i] = ((*n)->x) - 1735;
         SignalFactory::SignalInstance()->locateY[i] = ((*n)->y) - 1734;
         SignalFactory::SignalInstance()->m_PointArray[i][0] = SignalFactory::SignalInstance()->locateX[i];
         SignalFactory::SignalInstance()->m_PointArray[i][1] = SignalFactory::SignalInstance()->locateY[i];
         SignalFactory::SignalInstance()->m_PointArray[i][2] = 9;

//         px[i] = ((*n)->x) * 10;
//         py[i] = ((*n)->y) * 10;
     }
//     int index = 0;
//     for(unsigned long long j = 0; j < SignalFactory::SignalInstance()->m_PointNum - 1; j++)
//     {
//        index = DAPlan(px[j], py[j], px[j+1], py[j+1],index);
//     }
//     SignalFactory::SignalInstance()->locateX[index] = px[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 400;
//     SignalFactory::SignalInstance()->locateY[index] = py[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 1000;
//     SignalFactory::SignalInstance()->m_PointArray[index][0] = px[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 400;
//     SignalFactory::SignalInstance()->m_PointArray[index][1] = py[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 1000;
//     SignalFactory::SignalInstance()->m_PointArray[index][2] = 15.5;
//     SignalFactory::SignalInstance()->m_pointNum1 = index;
//     qDebug()<<'('<<px[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 400<<','<<py[SignalFactory::SignalInstance()->m_PointNum-1] * 0.1 - 1000<<')'<<endl;
// }

//    SignalFactory::SignalInstance()->isEnd = true;

        emit SignalFactory::SignalInstance()->PlanSuccessInfo();


}
int AlgoThread::DAPlan(int startx, int starty, int endx, int endy, int index)
{
    int rows = 21;
    int columns = 21;
    vector<vector<int>> maze(columns, vector<int>(rows));
    for(int i = 0; i < columns; i++)
    {
        for(int j = 0; j < rows; j++)
        {
            maze[i][j] = 0;     //0为可通行，1为障碍，初始化地图为全部可通行
        }
    }

    Astar dastar;
    dastar.InitAstar(maze);

    int x = 10 + endx - startx;
    int y = 10 + endy - starty;

    Point start(10, 10);
    Point end(x, y);

    //cout<<'('<<startx<<','<<starty<<')'<<endl;
    list<Point *> path = dastar.GetPath(start, end, false);
    list<Point *>::iterator n;
    unsigned long long i = 0;
    pointNum2 = path.size();
    m_PointNum2 = pointNum2;

    for(n = path.begin(); i < pointNum2-1; i++, ++n)
    {
//                 SignalFactory::SignalInstance()->locateX[i+index] = ((*n)->x) * 0.1 + startx * 0.1 - 401;
//                 SignalFactory::SignalInstance()->locateY[i+index] = ((*n)->y) * 0.1 + starty * 0.1 - 1001;
                 SignalFactory::SignalInstance()->m_PointArray[i+index][0] = ((*n)->x) * 0.1 + startx * 0.1 - 1735;
                 SignalFactory::SignalInstance()->m_PointArray[i+index][1] = ((*n)->y) * 0.1 + starty * 0.1 - 1734;
//                 SignalFactory::SignalInstance()->m_PointArray[i+index][2] = 15.5;
                 SignalFactory::SignalInstance()->m_PointArray[i+index][2] = 9;
        qDebug()<<'('<<((*n)->x) * 0.1 + startx * 0.1 - 1735<<','<<((*n)->y) * 0.1 + starty * 0.1 - 1734<<')'<<endl;
    }
    return (pointNum2 - 1 + index);
}
